/*
 * Robot.h
 *
 *      Author: Ben, Philippe, Simon en Tom
 */

#ifndef ROBOT_H_
#define ROBOT_H_

#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <thread>
#include <cmath>
#include "../wifi/package.h"
#include "../wifi/server.h"
#include "../lowlevel/CanBotLL.h"
#include "../lowlevel/Serial.h"
using namespace std;

typedef struct Position{
	int x;
	int y;
	float arc;
} position;

/*!  \class Robot
 	 \brief Robot is the High Level implementation on CanBot.
 	 	 	This class is responsible for the calculation of robot movements
*/

class Robot{
public:
	Robot();
	Robot(int tol_arc, int tol_distance);
	
	/**
	* Deze functie leest data van het netwerk team in naar de parameters van deze classe
	* @param data struct van het netwerk team
	* @see package.h
	*/
	void UpdateRobot(Package data);		//meegeven struct van het netwerk team
	
	/**
	* Deze functie leest data van het netwerk team in naar de parameters van deze classe
	* @param data struct van het netwerk team
	* @see package.h
	*/
	void UpdateTarget(Package data);		//meegeven struct van het netwerk team
	
	/**
	* Deze functie berekend op hoog niveau welke hoek gemaakt moet worden tot het target
	*/
	int CalcTurn();		//High level strategie
	
	/**
	* Deze functie herberekend deze hoek en geeft deze door naar het low level team
	* @param arc de hoek die gedraaid moet worden
	*/
	void Turn(int arc,CanBotLL *c);		//Low level strategie
	
	/**
	* Deze functie berekend op hoog niveau de afstand tot het target
	*/
	int CalcDistance();	//High level strategie
	
	/**
	* Deze functie herberekend de afstand en geeft deze door naar het low level team
	* @param distance de afstand die gereden moet worden
	*/
	void Drive(int distance,CanBotLL *c);	//Low level strategie
	
	/**
	* Destructor Robot
	*/
	//~Robot();
	
private:
	Position me;
	Position target;
	int tolerance_arc;
	int tolerance_distance;
	bool cargo;
};


#endif /* ROBOT_H_ */